/*
Compute elastic reactions using

Sigma = lambda  tr(E) + 2 mu E
E = 1/2 ((Grad U )^T+ Grad U)

This is linear material combined with linear geometry.
*/

#include "TetrahedronState.h"
#include "Mechanics.h"

static void LinearCalcStrain (TetrahedronState const*, Matrix3 * strain, Matrix3 const & disp);

/*
Compute STRESS from STRAIN. STRESS and STRAIN may be aliased to each
other.

(setq linear_constitutive_equation (+ Matrix3::trace Matrix3::scale Matrix3::add_diag))

COST: 15 flops
*/
static void LinearConstitutiveEquation (Matrix3 *stress, Matrix3 const &strain)
{
	Real tr = strain.Trace();

	if (stress != &strain)
		*stress = strain;

	(*stress) *= 2*lame_mu;

	stress->AddDiag ( lame_lambda * tr);
}

Real LinearElasticEnergy (Real,Real,Real)
{
	/*
	This does not apply, since linear elastic energy is not a function
	of the invariants.
	*/

	assert (false);
	return 0.0;
}

/*
Put the elastic force of tetrahedron ME in GRAD_VEC, using the
displacement in DISPLACEMENT_VEC.

compute minus elastic force for one tetrahedron, from a
displacement.

COST:

(+ linear_calc_strain linear_constitutive_equation Matrix3::multiply_mm
Matrix3::negate)

(+ 60 15 54 0)

129 flops.

*/
int LinearElasticForce (TetrahedronState const*me,
					  Matrix3 * forces,
					  Matrix3 const& displacement_mat)
{
	if (me->degenerate_b_)
		return 0;

	Matrix3 strain;
	Matrix3 &stress (strain);

	LinearCalcStrain (me, &strain, displacement_mat); 
	LinearConstitutiveEquation (&stress, strain);

	Matrix3::MultiplyMM (*forces, stress, me->invloc_transpose_); // 54

	Matrix3::Negate (*forces, *forces); // 9

	return 129;
}



/*
COST:

(setq linear_calc_strain (+  Matrix3::multiply_mm Matrix3::symmetrify))

60  flops
*/
static void LinearCalcStrain (TetrahedronState const *me,
					Matrix3 *strain_dest,
					Matrix3  const&displacement_mat) 
{
	if (me->degenerate_b_)
		return;

	Matrix3::MultiplyMM (*strain_dest, displacement_mat, me->inverse_location_);
	Matrix3::Symmetrify (*strain_dest,*strain_dest); 
}
